#include <avr/io.h>
#include <avr/interrupt.h>
#include "radio.h"
#include "sonar.h"
// -- Constant Defines -------------------------------------------------------
#define METRIC_CONVERSION_FACTOR 14 // The speed of sound at STP is 340m/2 ==34cm/ms = 13.6 inch/ms
//----------------------------------------------------------------------------
unsigned int DISTANCE = 0;
unsigned int HEIGHT = 0;
int timer16Started = 0;
int activityFlag = 0;
int isDown = 1;
unsigned int riseTime = 0;
unsigned int fallTime = 0;
unsigned int TIME; 
static int sonarNum[NUM_SONAR];

/*
* This function is necessary as the 16 bit timer uses 16 bit registers.
* However, the bus is only 8 bit, thus reading the register requires two reads.
* We want this to be atomic, thus we need to disable interrupts when reading
* the timer counter register.
*/
unsigned int readTimerCounter16() {
	unsigned char sreg;
	unsigned int i;
	sreg = SREG; // Save global interrupt flag
	cli(); // Disable interrupts
	i = ICR3; // Read counter 1 i returns the height
	SREG = sreg; // Restore the interrupt flag
	sei(); // Re-enable interrupts
	return i;
} // end-function

// ...........................................................................


/*
* Positive == 1 to enable the positive edge triggering,
* anything else enables negative port triggering.
*/
void setLevelTrigger(int positive) {
	if (positive) TCCR3B |= _BV(ICES3);
	else TCCR3B &= !_BV(ICES3);
} // end-function
// ...........................................................................

/*
* This function inverts the level triggering
* (positive -> negative, negative -> positive).
*/
void flipLevelTrigger() {
//	radio_print("flip level trigger\n");
	TCCR3B ^= _BV(ICES3);
} // end-function
// ...........................................................................


/*
* The timer interrupt handler. This gets called when a the current level trigger
* occurs.
*/
ISR (TIMER3_CAPT_vect) {
	PORTD |= 0xf0;
	radio_print("Calling ISR\r\n");
	if (( ( (TCCR3B) & (_BV(ICES3)) ) !=0)) {
		//radio_print("pos level event\r\n");
	// We encountered a positive level event

		flipLevelTrigger();
        riseTime = ICR3;
		//need to read ICR3 when it goes up, and when it goes down
		//then subtract one from the other to get the time. 

	
	} else {
		
		//radio_print("Negative level event\r\n");
		PORTD |= _BV(PORTD6);
		// We encountered a negative level event
		fallTime = ICR3;
		
		flipLevelTrigger();
        TIME = (fallTime - riseTime);
		radio_print("Time is : %d\r\n", TIME);

		if(isDown == 1) {
		    HEIGHT = (TIME*METRIC_CONVERSION_FACTOR) / 250;
			activityFlag = 1;
			isDown =0;
		} else {
			DISTANCE = (TIME*METRIC_CONVERSION_FACTOR) / 250;
			activityFlag = 1;
			isDown = 1;
		}
		
		//
	} // end-if
} // end-function
// ...........................................................................


//this function should really be called init_sonar
void initTimer() {

	radio_print("Calling initTimer\r\n");
	DDRC =~_BV(PORTC7);  //enable down sonar?
	
	DDRA =~_BV(PORTA7); // it may  be port A4 for the right sonar. 


	/* enable timer. The two sonars are using the same timer */
	TIMSK3 = (_BV(ICIE3)); //Enable ICP3 interrupts
	TCCR3B = (_BV(ICNC3)) | (_BV(ICES3)) | (_BV(CS31)); // Configure the initial
	
	sonarNum[SONAR_DOWN] = PORTC7;
	sonarNum[SONAR_RIGHT] = PORTA7;
	sei(); //enable interrupts
	timer16Started = 1; // Flag the timer as started.
} // end-function

/*
* Send a pulse to the down sonar, and resets the timer counter to 0.

*/
void sendDownPulse() {
	radio_print("Sending down pulse. \r\n");

	PORTC |= _BV(PORTC6); //trigger for sonar down

	Task_Next();

	//_delay_ms(10);
	PORTC &= ~_BV(PORTC6);
	// Reset the timer counter to 0
	TCNT3 = 0;

} // end-function

/*This function sends the pulse for the right sonar */
void sendRightPulse() {
	radio_print("Sending right pulse. \r\n");
	PORTA |= _BV(PORTA6); //assuming that the trigger for sonar right is on A6
	Task_Next();

	PORTA &=~(_BV(PORTA6));

	//Reset the timer counter to 0
	TCNT3 = 0;

}

void triggerSonar(int sonar) {
	if(sonar == sonarNum[SONAR_DOWN] ){
		sendDownPulse();
	} else {
		sendRightPulse();
	}
}

/*
* This method will query the range finder and will get the range.
* The returned value is in centimetres. -1 is returned if a timeout occurs.
*/
void range(int sonarDirection) {
	if (!timer16Started) {
		// Setup and start the 16 bit timer, if necessary
		initTimer();
	} // end-if

	isDown = sonarDirection;

	if(isDown == 1 ) {
		triggerSonar(PORTC7);
	}else {
		triggerSonar(PORTA7);
	}
	// Wait for a reply
} // end-function


/*
* This function will send out a ping, wait for the response, and then return it.
* Note that if the closest physical object is out-of-range, this method will
* return the maximum range, and will not return any sort of special flag or error.
* Be aware that you shouldn't call this function in a tight loop - the sonar hardware
* needs at least 10ms between pulses. Thus be sure to use a short wait before calling
* it again.
*/
void getRange() {
	radio_print("calling getRange\r\n");
	activityFlag = 0;

	if (!timer16Started) {
		// Setup and start the 16 bit timer, if necessary
		initTimer();
	} // end-if

//	isDown = sonarDirection;

	if(isDown == 1 ) {
		triggerSonar(PORTC7);
	}else {
		triggerSonar(PORTA7);
	}

//	range();

	radio_print("Rise Time: %u\r\n", riseTime);
	radio_print("Fall Time: %u\r\n", fallTime);

	radio_print("Time:  %u\r\n", TIME);


		
} // end-function

int getDistanceFromWall(){
	return DISTANCE;
}

int getHeightfromGround() {
	return HEIGHT;
}


/* This function disables both sonars */
void disableSonars(){

	PORTC &= ~(_BV(PORTC7)); //down sonar 
	PORTA &= ~(_BV(PORTA7)); //right sonar - it may be A4

}
